#include "pch.h"
#include "primitives.h"
#include <stdio.h>
#include <sstream>
#include <ostream>


using namespace DXBase;

float2 DXBase::vector2(float x, float y)
{
	float2 result;
	result.x = x;
	result.y = y;
	return result;
}
float3 DXBase::vector3(float x, float y, float z)
{
	float3 result;
	result.x = x;
	result.y = y;
	result.z = z;
	return result;
}
float4 DXBase::vector4(float x, float y, float z, float w)
{
	float4 result;
	result.x = x;
	result.y = y;
	result.z = z;
	result.w = w;
	return result;
}
float4 DXBase::vector4(float3& v, float w)
{
	float4 result;
	result.x = v.x;
	result.y = v.y;
	result.z = v.z;
	result.w = w;
	return result;
}

float2 DXBase::lerp(const float2& a, const float2& b, float t)
{
	return vector2(
			(1.0f-t)*a.x + t*b.x,
			(1.0f-t)*a.y + t*b.y);
}

float3 DXBase::lerp(const float3& a, const float3& b, float t)
{
	return vector3(
			(1.0f-t)*a.x + t*b.x,
			(1.0f-t)*a.y + t*b.y,
			(1.0f-t)*a.z + t*b.z);
}

float4 DXBase::lerp(const float4& a, const float4& b, float t)
{
	return vector4(
			(1.0f-t)*a.x + t*b.x,
			(1.0f-t)*a.y + t*b.y,
			(1.0f-t)*a.z + t*b.z,
			(1.0f-t)*a.w + t*b.w);
}


bool DXBase::operator==(const float4x4& a, const float4x4& b)
{
	auto pa = (float*)&a;
	auto pb = (float*)&b;
	for (int i=0; i<16; i++)
		if (*pa++ != *pb++)
			return false;
	return true;
}

float4x4 DXBase::matrix4x4(float value)
{
	float4x4 m;
	m.m00 = m.m01 = m.m02 = m.m03 = value;
	m.m10 = m.m11 = m.m12 = m.m13 = value;
	m.m20 = m.m21 = m.m22 = m.m23 = value;
	m.m30 = m.m31 = m.m32 = m.m33 = value;
	return m;
}
float4x4 DXBase::matrix4x4(float3& vx, float3& vy, float3& vz)
{
	float4x4 m;
	m.m00 = vx.x; m.m01 = vy.x; m.m02 = vz.x; m.m03 = 0;
	m.m10 = vx.y; m.m11 = vy.y; m.m12 = vz.y; m.m13 = 0;
	m.m20 = vx.z; m.m21 = vy.z; m.m22 = vz.z; m.m23 = 0;
	m.m30 = 0;    m.m31 = 0;    m.m32 = 0;    m.m33 = 1;
	return m;
}
float4x4 DXBase::matrix4x4(float3& vx, float3& vy, float3& vz, float3& origin)
{
	float4x4 m;
	m.m00 = vx.x; m.m01 = vy.x; m.m02 = vz.x; m.m03 = origin.x;
	m.m10 = vx.y; m.m11 = vy.y; m.m12 = vz.y; m.m13 = origin.y;
	m.m20 = vx.z; m.m21 = vy.z; m.m22 = vz.z; m.m23 = origin.z;
	m.m30 = 0;    m.m31 = 0;    m.m32 = 0;    m.m33 = 1;
	return m;
}
float4x4 DXBase::matrix4x4(
	float i11, float i12, float i13, float i14,
	float i21, float i22, float i23, float i24,
	float i31, float i32, float i33, float i34,
	float i41, float i42, float i43, float i44
	)
{
	float4x4 m;
	m.m00 = i11; m.m01 = i12; m.m02 = i13; m.m03 = i14;
	m.m10 = i21; m.m11 = i22; m.m12 = i23; m.m13 = i24;
	m.m20 = i31; m.m21 = i32; m.m22 = i33; m.m23 = i34;
	m.m30 = i41; m.m31 = i42; m.m32 = i43; m.m33 = i44;
	return m;
}

float4x4 DXBase::transpose(float4x4& m)
{
	return matrix4x4(
		m.m00, m.m10, m.m20, m.m30,
		m.m01, m.m11, m.m21, m.m31,
		m.m02, m.m12, m.m22, m.m32,
		m.m03, m.m13, m.m23, m.m33
		);
}

float4x4 DXBase::mul(float4x4& m1,float4x4& m2)
{
	float4x4 mOut = matrix4x4();
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
			for (int k = 0; k < 4; k++)
				at(mOut, i, j) += at(m1,i,k) * at(m2,k,j);
	return mOut;
}


float4x4 DXBase::identity()
{
	return matrix4x4(
		1,0,0,0,
		0,1,0,0,
		0,0,1,0,
		0,0,0,1
	);
}

float4x4 DXBase::translation(float x, float y, float z)
{
	return matrix4x4(
		1,0,0,x,
		0,1,0,y,
		0,0,1,z,
		0,0,0,1
	);
}
float4x4 DXBase::translation(float3& v)
{
	return matrix4x4(
		1,0,0,v.x,
		0,1,0,v.y,
		0,0,1,v.z,
		0,0,0,1
	);
}

float4x4 DXBase::scale(float x, float y, float z)
{
	return matrix4x4(
		x,0,0,0,
		0,y,0,0,
		0,0,z,0,
		0,0,0,1
	);
}
float4x4 DXBase::scale(float3& v)
{
	return matrix4x4(
		v.x,0,0,0,
		0,v.y,0,0,
		0,0,v.z,0,
		0,0,0,1
	);
}


float4x4 DXBase::rotationX(float degreeX)
{
	return QuaternionOp::Rotation(QuaternionOp::FromRotation(vector4(1,0,0,degreeX)));
	//float angleInRadians = degreeX * (PI_F / 180.0f);
	//float sinAngle = sinf(angleInRadians);
	//float cosAngle = cosf(angleInRadians);
	//return matrix4x4(
	//	1,0,0,0,
	//	0,cosAngle,-sinAngle,0,
	//	0,sinAngle,cosAngle,0,
	//	0,0,0,1
	//);
}

float4x4 DXBase::rotationY(float degreeY)
{
	return QuaternionOp::Rotation(QuaternionOp::FromRotation(vector4(0,1,0,degreeY)));
	//float angleInRadians = degreeY * (PI_F / 180.0f);
	//float sinAngle = sinf(angleInRadians);
	//float cosAngle = cosf(angleInRadians);
	//return matrix4x4(
	//	cosAngle,0,sinAngle,0,
	//	0,1,0,0,
	//	-sinAngle,0,cosAngle,0,
	//	0,0,0,1
	//);
}

float4x4 DXBase::rotationZ(float degreeZ)
{
	return QuaternionOp::Rotation(QuaternionOp::FromRotation(vector4(0,0,1,degreeZ)));
	//float angleInRadians = degreeZ * (PI_F / 180.0f);
	//float sinAngle = sinf(angleInRadians);
	//float cosAngle = cosf(angleInRadians);
	//return matrix4x4(
	//	cosAngle,-sinAngle,0,0,
	//	sinAngle,cosAngle,0,0,
	//	0,0,1,0,
	//	0,0,0,1
	//);
}

// 3D Rotation matrix for an arbitrary axis specified by x, y and z
float4x4 DXBase::rotation(float4& rot)
{
	auto q = QuaternionOp::FromRotation(rot);
	return QuaternionOp::Rotation(q);
	//auto axis = normalize(DXBase::vector3(rot.x, rot.y, rot.z));
	//float sinAngle = sinf(rot.w);
	//float cosAngle = cosf(rot.w);
	//float oneMinusCosAngle = 1 - cosAngle;

	//float4x4 mOut;

	//mOut.m00 = 1.0f + oneMinusCosAngle * (axis.x * axis.x - 1.0f);
	//mOut.m01 = axis.z * sinAngle + oneMinusCosAngle * axis.x * axis.y;
	//mOut.m02 = -axis.y * sinAngle + oneMinusCosAngle * axis.x * axis.z;
	//mOut.m03 = 0.0f;

	//mOut.m10 = -axis.z * sinAngle + oneMinusCosAngle * axis.y * axis.x;
	//mOut.m11 = 1.0f + oneMinusCosAngle * (axis.y * axis.y - 1.0f);
	//mOut.m12 = axis.x * sinAngle + oneMinusCosAngle * axis.y * axis.z;
	//mOut.m13 = 0.0f;

	//mOut.m20 = axis.y * sinAngle + oneMinusCosAngle * axis.z * axis.x;
	//mOut.m21 = -axis.x * sinAngle + oneMinusCosAngle * axis.z * axis.y;
	//mOut.m22 = 1.0f + oneMinusCosAngle * (axis.z * axis.z - 1.0f);
	//mOut.m23 = 0.0f;

	//mOut.m30 = 0.0f;
	//mOut.m31 = 0.0f;
	//mOut.m32 = 0.0f;
	//mOut.m33 = 1.0f;

	//return mOut;
}

float4x4 DXBase::rotation(float3& v0, float3& v1)
{
	v0 = normalize(v0);
	v1 = normalize(v1);
	auto v2 = cross(v0, v1);
	auto sina = length(v2);
	if (sina < 0.000001)
		return identity();
	auto cosa = dot(v0, v1);
	auto a = atan2(sina, cosa);
	return rotation(DXBase::vector4(v2, a));
}
quaternion DXBase::getquaternion(float3& v0, float3& v1)
{
	return QuaternionOp::FromRotation(getrotation(v0, v1));
}
float4 DXBase::getrotation(float3& v0, float3& v1)
{
	v0 = normalize(v0);
	v1 = normalize(v1);
	auto axis = DXBase::vector4(cross(v0, v1), 0);
	auto sina = length(axis);
	if (sina < 0.000001)
		return vector4(1,0,0,0);

	axis = axis / sina;
	auto cosa = dot(v0, v1);
	float w = static_cast<float>(atan2(sina, cosa));
	axis.w = RAD2DEG(w);
	return axis;
}

float4 DXBase::operator*(float4x4& a, float4& b)
{
	float4 r = vector4(0, 0, 0, 0);
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
			at(r, i) += at(a,i,j) * at(b,j);
	return r;
}

static void DumpDescription(const SquareMatrix<float>& m, const wchar_t * msg)
{
	std::wostringstream ss;
	ss << L"matrix: " << msg << "\r\n";
	for (int i=0; i<m.size; i++)
	{
		for (int j=0; j<m.size; j++)
			ss << L"\t" << m.at(i, j);
		ss << L"\r\n";
	}
	OutputDebugString(ss.str().c_str());
}

static SquareMatrix<float> GetSquareMatrix(const float4x4& m)
{
	SquareMatrix<float> result(4);
	auto src = (float*)&m;
	auto dst = (float*)result.data;
	memcpy(dst, src, 16 * sizeof(float));
	return result;
}
static float4x4 GetMatrix(const SquareMatrix<float>& m)
{
	float4x4 result;
	auto src = (float*)m.data;
	auto dst = (float*)&result;
	memcpy(dst, src, 16 * sizeof(float));
	return result;
}
float DXBase::determinant(const float4x4& m)
{
	return GetSquareMatrix(m).Determinant();
}
float4x4 DXBase::invert(const float4x4& m)
{
	return GetMatrix(GetSquareMatrix(m).Invert());
}

bool DXBase::decompose(float4x4& m, float3& translation, float4& rot, float3& scale)
{
	quaternion q;
	if (!decompose(m, translation, q, scale))
		return false;
	rot = QuaternionOp::ToRotation(q);
	return true;
}
bool DXBase::decompose(float4x4& m, float3& translation, quaternion& q, float3& scale)
{
	if (at(m, 3, 0) || at(m, 3, 1) || at(m, 3, 2))
		return false;

	translation.x = m.m03;
	translation.y = m.m13;
	translation.z = m.m23;

	scale.x = sqrtf(m.m00 * m.m00 + m.m10 * m.m10 + m.m20 * m.m20);
	scale.y = sqrtf(m.m01 * m.m01 + m.m11 * m.m11 + m.m21 * m.m21);
	scale.z = sqrtf(m.m02 * m.m02 + m.m12 * m.m12 + m.m22 * m.m22);
	
	q = QuaternionOp::Extract(m);

	return true;
}


float4x4 QuaternionOp::Rotation(quaternion q)
{
	return matrix4x4(
		vector3(1-2*(q.y*q.y+q.z*q.z), 2*(q.x*q.y-q.z*q.w), 2*(q.x*q.z+q.y*q.w)), 
		vector3(2*(q.x*q.y+q.z*q.w), 1-2*(q.x*q.x+q.z*q.z), 2*(q.y*q.z-q.x*q.w)), 
		vector3(2*(q.x*q.z-q.y*q.w), 2*(q.y*q.z+q.x*q.w), 1-2*(q.x*q.x+q.y*q.y))
	);
}
quaternion QuaternionOp::Extract(float4x4 m)
{
	float3 s;
	s.x = sqrtf(m.m00 * m.m00 + m.m10 * m.m10 + m.m20 * m.m20);
	s.y = sqrtf(m.m01 * m.m01 + m.m11 * m.m11 + m.m21 * m.m21);
	s.z = sqrtf(m.m02 * m.m02 + m.m12 * m.m12 + m.m22 * m.m22);
	for(int i=0; i<3; i++)
	{
		at(m,i,0) = at(m,i,0) / s.x;
		at(m,i,1) = at(m,i,1) / s.y;
		at(m,i,2) = at(m,i,2) / s.z;
	}

	quaternion q;
	float S;
	auto T = 1+m.m00+m.m11+m.m22;
	if (T > 0.0000001) 
	{
		S = sqrtf(T) * 2;
		q.x = (m.m21-m.m12)/S;
		q.y = (m.m02-m.m20)/S;
		q.z = (m.m10-m.m01)/S;
		q.w = S / 4;
	}
	else if ( m.m00 > m.m11 && m.m00 > m.m22 ) 
	{	// Column 0: 
        S  = sqrtf( 1.0f + m.m00 - m.m11 - m.m22 ) * 2;
		q.x = 0.25f * S;
        q.y = (m.m10 + m.m01 ) / S;
        q.z = (m.m02 + m.m20 ) / S;
        q.w = (m.m21 - m.m12 ) / S;
    }
	else if ( m.m11 > m.m22 )
	{	// Column 1: 
        S  = sqrtf( 1.0f + m.m11 - m.m00 - m.m22 ) * 2;
        q.x = (m.m10 + m.m01 ) / S;
        q.y = 0.25f * S;
        q.z = (m.m21 + m.m12 ) / S;
        q.w = (m.m02 - m.m20 ) / S;
    }
	else 
	{	// Column 2:
        S  = sqrtf( 1.0f + m.m22 - m.m00 - m.m11 ) * 2;
        q.x = (m.m02 + m.m20 ) / S;
        q.y = (m.m21 + m.m12 ) / S;
        q.z = 0.25f * S;
        q.w = (m.m10 - m.m01 ) / S;
    }
	return q;
}
quaternion QuaternionOp::FromRotation(float4 rot)
{
	float w = DEG2RAD(rot.w);
	quaternion q;
	float sin_a = sinf(w/2);
	float cos_a = cosf(w/2);
	float len = sqrtf(rot.x*rot.x + rot.y*rot.y + rot.z*rot.z);
	if (len < 0.0000001)
		return cquaternion(0,0,0,1);
	q.x = rot.x / len * sin_a;
	q.y = rot.y / len * sin_a;
	q.z = rot.z / len * sin_a;
	q.w = cos_a;
	return q;
}
float4 QuaternionOp::ToRotation(quaternion q)
{
	float4 rot;
	float cos_a = q.w;
	float sin_a = sqrt(1 - cos_a * cos_a);
	float w = 2 * atan2f(sin_a, cos_a);
	rot.w = RAD2DEG(w);
	if (sin_a < 0.00005) sin_a = 1;
	rot.x = q.x / sin_a;
	rot.y = q.y / sin_a;
	rot.z = q.z / sin_a;
	return rot;
}
quaternion QuaternionOp::Normalize(quaternion q)
{
	auto len = sqrtf(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
	q.x/=len;
	q.y/=len;
	q.z/=len;
	q.w/=len;
	return q;
}
quaternion QuaternionOp::Mul(quaternion q1, quaternion q2)
{
	auto v1 = vector3(q1.x, q1.y, q1.z);
	auto w1 = q1.w;
	auto v2 = vector3(q2.x, q2.y, q2.z);
	auto w2 = q2.w;
	auto result = cquaternion(
		w1 * v2 + w2 * v1 + cross(v1, v2),
		w1 * w2 - dot(v1, v2)
		);
	return Normalize(result);
}
float3 QuaternionOp::Mul(quaternion q, float3 p)
{
	auto len = length(p);
	quaternion qp = cquaternion(p.x, p.y, p.z, 0);
	quaternion tmp = Invert(q);
	tmp = Mul(qp, tmp);
	tmp = Mul(q, tmp);
	return vector3(tmp.x * len, tmp.y * len, tmp.z * len);
}
quaternion QuaternionOp::Invert(quaternion q)
{
	auto len = 1 / sqrtf(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w);
	return cquaternion(- len * q.x,- len * q.y,- len * q.z, len * q.w);
}

#define SLERP_TO_LERP_SWITCH_THRESHOLD 0.01f
quaternion QuaternionOp::Slerp(quaternion q1, quaternion q2, float alpha)
{
  float q1Weight, q2Weight, difference;
  quaternion result;
  
  difference = ((q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z) + (q1.w * q2.w));
  if ((1.0f - fabs(difference)) > SLERP_TO_LERP_SWITCH_THRESHOLD) {
    float theta, oneOverSinTheta;
    
    theta = acosf(fabs(difference));
    oneOverSinTheta = (1.0f / sin(theta));
    q1Weight = (sinf(theta * (1.0f - alpha)) * oneOverSinTheta);
    q2Weight = (sinf(theta * alpha) * oneOverSinTheta);
    if (difference < 0.0f) {
      q1Weight = -q1Weight;
    }
  } else {
    q1Weight = (1.0f - alpha);
    q2Weight = alpha;
  }
  result.x = (q1.x * q1Weight) + (q2.x * q2Weight);
  result.y = (q1.y * q1Weight) + (q2.y * q2Weight);
  result.z = (q1.z * q1Weight) + (q2.z * q2Weight);
  result.w = (q1.w * q1Weight) + (q2.w * q2Weight);
  return Normalize(result);
}


float3x2 DXBase::matrix2d(float m11, float m12, float m21, float m22, float m31, float m32)
{
	float3x2 m;
	m.m11 = m11;
	m.m12 = m12;
	m.m21 = m21;
	m.m22 = m22;
	m.m31 = m31;
	m.m32 = m32;
	return m;
}
float3x2 DXBase::identity2d() 
{
	return matrix2d(
		1, 0,
		0, 1,
		0, 0
		);
}
float3x2 DXBase::rotate2d(float degrees, const float2& center)
{
	float cosa = cosf(DEG2RAD(degrees));
	float sina = sinf(DEG2RAD(degrees));
	return matrix2d(
		cosa, sina,
		-sina, cosa,
		- cosa * center.x + sina * center.y + center.x, - sina * center.x - cosa * center.y + center.y
		);
}
float3x2 DXBase::scale2d(float sx, float sy, const float2& center)
{
	return matrix2d(
		sx, 0,
		0, sy, 
		center.x * (1 - sx), center.y * (1 - sy)
		);
}
float3x2 DXBase::skew2d(float degreesX, float degreesY, const float2& center)
{
	float skewx = tanf(DEG2RAD(degreesX));
	float skewy = tanf(DEG2RAD(degreesY));
	return matrix2d(
		1, skewy,
		skewx, 1, 
		- skewx * center.y, - skewy * center.x
		);
}
float3x2 DXBase::translate2d(float dx, float dy)
{
	return matrix2d(
		1, 0,
		0, 1, 
		dx, dy
		);
}
float3x2 DXBase::mul(const float3x2& a, const float3x2& b)
{
	return matrix2d(
		a.m11 * b.m11 + a.m12 * b.m21, a.m11 * b.m12 + a.m12 * b.m22,
		a.m21 * b.m11 + a.m22 * b.m21, a.m21 * b.m12 + a.m22 * b.m22, 
		a.m31 * b.m11 + a.m32 * b.m21 + b.m31, a.m31 * b.m12 + a.m32 * b.m22 + b.m32
		);
}
float2 DXBase::mul(const float3x2& m, const float2& p)
{
	return vector2(
		m.m11 * p.x + m.m21 * p.y + m.m31,
		m.m12 * p.x + m.m22 * p.y + m.m32
		);
}
float3x2 DXBase::invert(const float3x2& m)
{
	float det = m.m11 * m.m22 - m.m12 * m.m21;
	float3x2 mi;
	mi.m11 = m.m22 / det;
	mi.m12 = -m.m12 / det;
	mi.m21 = -m.m21 / det;
	mi.m22 = m.m11 / det;
	mi.m31 = - mi.m11 * m.m31 - mi.m21 * m.m32;
	mi.m32 = - mi.m12 * m.m31 - mi.m22 * m.m32;
	return mi;
}
